#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include "Vector3.h"

/* compile with -lpthread and -llo */

#include <pthread.h>
#include "lo/lo.h"

#define UPDATE_PATH "/pfs/listener/0001/updateAxes"
#define INFINITY_COMMAND "/infinity/target"
#define PLC_NUMBER 7
#define LISTEN_PORT "25669"
#define LOCAL_PORT "25670"
#define LOOPS 500
#define NUM_AXES 8

void * server_thread( void * arg );
void * position_reporter( void * arg);
void * target_reporter( void * arg);

int emergencystop = 0;
int count = 0;
int updatecount = 0;
int targetcount = 0;
int done = 0;

int speed = 0;
int aspeed = 0;
int bspeed = 0;

lo_address t;

pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

int axis_position[NUM_AXES] = { 1 };
int axis_status[NUM_AXES] = { 0 };
int axis_error[NUM_AXES] = { 0 };
int axis_number[NUM_AXES] = { 0 };
int axis_load[NUM_AXES] = { 0 };

int axis_target[3] = { 0 };

lo_address local_broadcast;

// A 62 B 63 C 66 D 67


Vector3 wincha;
Vector3 winchb;

int amin = 27888;
int bmin = 27199;

int amax = 37597;
int bmax = 37580;

int aoffset = 2302;
int boffset = 2346;

int sn = 40000;

Vector3 position;
Vector3 starting;
Vector3 target;

Vector3 A,B;

void error(int num, const char *m, const char *path);

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int main() {
	pthread_t tid[3];
	int rc;

  t = lo_address_new("192.168.2.57", "25666");

	local_broadcast = lo_address_new("192.168.2.89", LOCAL_PORT);

	wincha.x = 0;
	wincha.y = 0;
	wincha.z = 0;

	winchb.x = 0;
	winchb.y = 0;
	winchb.z = 12192;

	position.x = 0;
	position.y = 5000;
	position.z = 6000;

  target.x = 0;
  target.y = 0;
  target.z = 0;

	starting.x = 0;
	starting.y = 5000;
	starting.z = 6000;

	printf("Creating threads\n");
	rc = pthread_create( &tid[0], NULL, server_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[1], NULL, position_reporter, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[2], NULL, target_reporter, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	printf("Threads created\n");

	pthread_join( tid[0], NULL );
	printf( "Terminating.\n" );
	pthread_mutex_destroy(&mutex);

	return 0;
}

void loadstate(int state){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "ii", sn, state);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, "/pfs/ur/0006/loadState", message); 
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void startplayback(char* path){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "i", sn);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message); 
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void sendtoposition(char* path, int axis, int position){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "iii", sn, axis, position);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message); 
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void setspeed(char* path, int speed){
    //sn++;
    if(sn > 65535)
      sn = 0;
    lo_message message = lo_message_new();
    lo_message_add(message, "ii", sn, speed);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, path, message); 
    lo_send_bundle(t, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);
}

void error(int num, const char *msg, const char *path)
{
	printf("liblo server error %d in path %s: %s\n", num, path, msg);
	fflush(stdout);
}

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	int PLC = argv[0]->i;
	if(PLC != PLC_NUMBER)
		return 1;
  pthread_mutex_lock(&mutex);
  int i;
  for(i = 0; i < NUM_AXES; i++){
    axis_number[i] = argv[(i*5)+2]->i;
    axis_position[i] = argv[(i*5)+3]->i;
    axis_load[i] = argv[(i*5)+4]->i;
    axis_status[i] = argv[(i*5)+5]->i;
    axis_error[i] = argv[(i*5)+6]->i;
  }

  updatecount++;
  pthread_mutex_unlock(&mutex);
	fflush(stdout);

	return 1;
}

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  target.x = argv[0]->f;
  target.x = 0;
  target.y = argv[1]->f;
  target.z = argv[2]->f;
  if(target.z > 9278)
    target.z = 9278;
  if(target.z < 3558)
    target.z = 3558;
  if(target.y > 5764)
    target.y = 5764;
  if(target.y < 2616)
    target.y = 2616;
  speed = argv[3]->i;
  targetcount++;

  int a = (int)vecLength(vecSub(wincha, target));
  int b = (int)vecLength(vecSub(winchb, target));
  a = a - aoffset;
  b = b - boffset;
  a = amax - a;
  b = bmax - b;
  axis_target[0] = a;
  axis_target[1] = b;
  pthread_mutex_unlock(&mutex);
	return 1;  
}

void * position_reporter(void * arg){
	int i;
	while(1){
		pthread_mutex_lock(&mutex);
		position.x = starting.x;
		position.y = starting.y;
		position.z = starting.z;
		for( i = 0; i < LOOPS; i++){
			A = vecSub(position, wincha);
			B = vecSub(position, winchb);
			A = vecAdd(vecMult(A,((double)(amax-axis_position[1]+aoffset))/vecLength(A)), wincha);
			B = vecAdd(vecMult(B,((double)(bmax-axis_position[6]+boffset))/vecLength(B)), winchb);
			position = vecAdd(A,B);
			position = vecDiv(position,2);
		}
		count++;
		pthread_mutex_unlock(&mutex);
		if (lo_send(local_broadcast, "/infinity/update", "fff", position.x, position.y, position.z) == -1) {
			printf("OSC error %d: %s\n", lo_address_errno(local_broadcast), lo_address_errstr(local_broadcast));
		}
		usleep(50000);
	}
}

void * target_reporter(void * arg){
  while(1){
    pthread_mutex_lock(&mutex);
    if(axis_target[0] < amax && axis_target[0] > amin && axis_target[1] < bmax && axis_target[1] > bmin){
      double high;
      int a = abs(axis_target[0]-axis_position[1]);
      int b = abs(axis_target[1]-axis_position[6]);
      if(a >= b)
        high = a;
      if(b > a)
        high = b;
      if(high > 0){
        aspeed = a/high*100;
        bspeed = b/high*100;
      }
      if(vecLength(vecSub(target,position)) > 50){
        setspeed("/pfs/ur/0006/pb/1/setSpeed", (int)ceil(speed * ((double)aspeed/100.0)));
        setspeed("/pfs/ur/0006/pb/2/setSpeed", (int)ceil(speed * ((double)bspeed/100.0)));
        sendtoposition("/pfs/ur/0006/pb/1/axis/0002/setposition", 2, axis_target[0]);
        sendtoposition("/pfs/ur/0006/pb/2/axis/0007/setposition", 7, axis_target[1]);
      }
      else{
        setspeed("/pfs/ur/0006/pb/1/setSpeed", 0);
        setspeed("/pfs/ur/0006/pb/2/setSpeed", 0);
      }
    }
    else{
      printf("OUT OF RANGE\n");
    }
    pthread_mutex_unlock(&mutex);
    usleep(200000);
  }
}

void * server_thread( void * arg ) {
	printf("In thread\n");
	/* start a new server on port LISTEN_PORT */
	lo_server_thread st = lo_server_thread_new(LISTEN_PORT, error);

	/* add function to catch UPDATE_PATH OSC commands */
	lo_server_thread_add_method(st, UPDATE_PATH, NULL, update_handler, NULL);

  lo_server_thread_add_method(st, INFINITY_COMMAND, "fffi", target_handler, NULL);

  lo_server_thread_add_method(st, "/stop", NULL, emergency_handler, NULL);

  lo_server_thread_add_method(st, "/state", "i", state_handler, NULL);

  lo_server_thread_add_method(st, "/playback", "i", playback_handler, NULL);

	lo_server_thread_start(st);

	while (!done) {
		//Polling Loop
		usleep(1000000);
		pthread_mutex_lock(&mutex);
    int i;
    if(updatecount == 0){
      printf("NO UPDATES!\n");
    }
    else{
      if(target.x == 0 && target.y == 0 && target.z == 0){
        target.x = position.x;
        target.y = position.y;
        target.z = position.z;

        int a = (int)vecLength(vecSub(wincha, target));
        int b = (int)vecLength(vecSub(winchb, target));
        a = a - aoffset;
        b = b - boffset;
        a = amax - a;
        b = bmax - b;

        axis_target[0] = a;
        axis_target[1] = b;
      }
      for(i = 0; i < NUM_AXES; i++){
        printf("Axis: %d Pos: %d Load: %d Status: %d Error: %d\n", axis_number[i], axis_position[i], axis_load[i], axis_status[i], axis_error[i]);
      }
      printf("Position X: %f Y: %f Z: %f count: %d\n", position.x, position.y, position.z, count);
      printf("Target X: %f Y: %f Z: %f count: %d\n", target.x, target.y, target.z, targetcount);
      printf("A length: %d B length: %d C length: %d \n", (amax-axis_position[1]+aoffset), (bmax-axis_position[6]+boffset), 0);
      printf("PREDICTED: A length: %d B length %d C length %d\n",
        (int)vecLength(vecSub(wincha, position)),
        (int)vecLength(vecSub(winchb, position)),
        0);
      printf("Target: A length: %d B length %d C length %d\n", (amax-axis_target[0]+aoffset), (bmax-axis_target[1]+boffset), 0);
      printf("Aspeed: %d Bspeed: %d\n",(int)ceil(50.0 * ((double)aspeed/100.0)),(int)ceil(50.0 * ((double)bspeed/100.0)));
      updatecount = 0;
      targetcount = 0;
      count = 0;
    }
		pthread_mutex_unlock(&mutex);
	}

	lo_server_thread_free(st);

	return 0;
}

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  printf("GOT!\n");
  pthread_mutex_lock(&mutex);
  while(1){
    printf("EMERGENCY STOP!\n");
    setspeed("/pfs/ur/0006/pb/1/setSpeed", 0);
    setspeed("/pfs/ur/0006/pb/2/setSpeed", 0);
    sendtoposition("/pfs/ur/0006/pb/1/axis/0002/setposition", 2, axis_position[1]);
    sendtoposition("/pfs/ur/0006/pb/2/axis/0007/setposition", 7, axis_position[6]);
    usleep(50000);
  }
}

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  printf("STATE LOADING\n");
  loadstate(argv[0]->i);
  pthread_mutex_unlock(&mutex);
}

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  if(argv[0]->i<1 || argv[0]->i > 6)
    return 0;
  char str[30];
  sprintf(str, "/pfs/ur/0006/pb/%d/start", argv[0]->i);
  startplayback(str);
  pthread_mutex_unlock(&mutex);
}
